import Foundation
import Combine

// AI命令执行器
class AICommandExecutor: ObservableObject {
    // MARK: - 属性
    @Published var isExecuting: Bool = false
    @Published var currentCommandIndex: Int = 0
    @Published var totalCommands: Int = 0
    @Published var executionProgress: Double = 0.0
    
    // 执行状态
    private var commandQueue: [CarControlCommand] = []
    private var currentTimer: Timer?
    private var executionTimer: Timer?
    
    // 回调
    var onCommandComplete: ((Int, CarControlCommand) -> Void)?
    var onExecutionComplete: (() -> Void)?
    var onExecutionError: ((String) -> Void)?
    
    // 取消器
    private var cancellables = Set<AnyCancellable>()
    
    // 小车控制服务引用
    private weak var carControlService: CarControlService?
    private weak var tcpCarService: TCPCarService?
    
    // MARK: - 初始化
    init(carControlService: CarControlService? = nil, tcpCarService: TCPCarService? = nil) {
        self.carControlService = carControlService
        self.tcpCarService = tcpCarService
    }
    
    // MARK: - 服务设置方法
    func setCarControlService(_ service: CarControlService?) {
        self.carControlService = service
    }
    
    func setTCPCarService(_ service: TCPCarService?) {
        self.tcpCarService = service
    }
    
    // MARK: - 公共方法
    func executeCommands(_ commands: [CarControlCommand]) {
        guard !commands.isEmpty else {
            onExecutionError?("没有可执行的命令")
            return
        }
        
        // 停止当前执行
        stopExecution()
        
        // 设置命令队列
        commandQueue = commands
        currentCommandIndex = 0
        totalCommands = commands.count
        executionProgress = 0.0
        isExecuting = true
        
        print("[AICommandExecutor] 开始执行 \(commands.count) 个命令")
        
        // 开始执行第一个命令
        executeNextCommand()
    }
    
    func stopExecution() {
        resetExecutionState()
        
        // 发送停止命令
        let stopCommand = CarControlCommand(direction: .stop, speed: .stop)
        sendCommand(stopCommand)
        
        print("[AICommandExecutor] 执行已停止")
        onExecutionComplete?() // 确保界面能复位
    }
    
    func pauseExecution() {
        guard isExecuting else { return }
        
        currentTimer?.invalidate()
        currentTimer = nil
        executionTimer?.invalidate()
        executionTimer = nil
        
        print("[AICommandExecutor] 执行已暂停")
    }
    
    func resumeExecution() {
        guard isExecuting && currentCommandIndex < commandQueue.count else { return }
        
        executeNextCommand()
        print("[AICommandExecutor] 执行已恢复")
    }
    
    // MARK: - 私有方法
    private func executeNextCommand() {
        guard currentCommandIndex < commandQueue.count else {
            completeExecution()
            return
        }
        
        let command = commandQueue[currentCommandIndex]
        print("[AICommandExecutor] 执行命令 \(currentCommandIndex + 1)/\(totalCommands): \(command.direction.rawValue) \(command.speed.description)")
        
        // 发送命令
        sendCommand(command)
        
        // 更新进度
        executionProgress = Double(currentCommandIndex) / Double(totalCommands)
        
        // 通知命令开始执行
        onCommandComplete?(currentCommandIndex, command)
        
        // 如果有持续时间，设置定时器
        if let duration = command.duration, duration > 0 {
            // 使用高精度定时器，最小支持0.1秒
            let preciseDuration = max(0.1, duration)
            currentTimer = Timer.scheduledTimer(withTimeInterval: preciseDuration, repeats: false) { [weak self] _ in
                self?.commandCompleted()
            }
        } else {
            // 没有持续时间，立即完成
            DispatchQueue.main.asyncAfter(deadline: .now() + 0.05) { [weak self] in  // 减少默认延迟
                self?.commandCompleted()
            }
        }
    }
    
    private func commandCompleted() {
        currentCommandIndex += 1
        
        // 更新进度
        executionProgress = Double(currentCommandIndex) / Double(totalCommands)
        
        // 执行下一个命令 - 减少命令间隔以支持快速动作变化
        DispatchQueue.main.asyncAfter(deadline: .now() + 0.05) { [weak self] in  // 从0.2秒减少到0.05秒
            self?.executeNextCommand()
        }
    }
    
    private func completeExecution() {
        resetExecutionState()
        
        // 发送停止命令确保小车停止
        let stopCommand = CarControlCommand(direction: .stop, speed: .stop)
        sendCommand(stopCommand)
        
        print("[AICommandExecutor] 所有命令执行完成")
        onExecutionComplete?()
    }
    
    private func resetExecutionState() {
        isExecuting = false
        currentTimer?.invalidate()
        currentTimer = nil
        executionTimer?.invalidate()
        executionTimer = nil
        
        // 重置进度状态
        currentCommandIndex = 0
        totalCommands = 0
        executionProgress = 0.0
        commandQueue.removeAll()
    }
    
    private func sendCommand(_ command: CarControlCommand) {
        print("[AICommandExecutor] 发送命令: \(command.direction.rawValue) \(command.speed.description) 持续时间: \(command.duration ?? 0)秒")
        
        // 优先使用 CarControlService
        if let carControlService = carControlService {
            carControlService.sendCommand(command)
        } else if let tcpCarService = tcpCarService {
            // 如果 CarControlService 不可用，使用 TCPCarService
            // 需要将 CarControlCommand 转换为 TCP 协议命令
            sendTCPCommand(command)
        } else {
            print("[AICommandExecutor] 错误：没有可用的控制服务")
        }
    }
    
    private func sendTCPCommand(_ command: CarControlCommand) {
        // 根据命令类型选择协议
        if command.direction == .horn {
            sendHornCommand(command)
        } else if shouldUseDirectionControl(command) {
            sendDirectionCommand(command)
        } else {
            sendSpeedCommand(command)
        }
    }
    
    private func shouldUseDirectionControl(_ command: CarControlCommand) -> Bool {
        // 对于明确的方向命令（旋转、平移、停止），使用方向控制协议
        switch command.direction {
        case .left, .right, .leftShift, .rightShift, .stop:
            return true
        case .forward, .backward:
            return false // 前进后退使用速度控制更精确
        case .horn:
            return false // 鸣笛使用专用协议
        }
    }
    
    private func sendHornCommand(_ command: CarControlCommand) {
        // 获取鸣笛参数，如果没有则使用默认值
        let hornParams = command.hornParameters ?? .beat
        
        // 构建鸣笛命令数据
        let data: [UInt8] = [hornParams.status, hornParams.delayLevel]
        let hornCmd = CarProtocolBuilder.buildCommand(
            carType: 0x01,
            functionCode: 0x13,  // 鸣笛功能码
            data: data,
            fixedLength: 0x06
        )
        
        print("[AICommandExecutor] 发送鸣笛命令: status=\(hornParams.status), delay=\(hornParams.delayLevel), 协议: \(String(data: hornCmd, encoding: .utf8) ?? "编码失败")")
        
        tcpCarService?.sendData(hornCmd) { success in
            print("[AICommandExecutor] 鸣笛命令发送\(success ? "成功" : "失败")")
        }
    }
    
    private func sendDirectionCommand(_ command: CarControlCommand) {
        // 使用方向控制协议 (功能码 0x15)，与人工控制保持一致
        let directionValue = getDirectionValue(command.direction)
        
        let directionCmd = CarProtocolBuilder.buildCommand(
            carType: 0x01,
            functionCode: 0x15,  // 方向控制功能码
            data: [directionValue]
        )
        
        print("[AICommandExecutor] 发送方向命令: \(command.direction.rawValue)(\(directionValue)), 协议: \(String(data: directionCmd, encoding: .utf8) ?? "编码失败")")
        
        tcpCarService?.sendData(directionCmd) { success in
            print("[AICommandExecutor] 方向命令发送\(success ? "成功" : "失败")")
        }
    }
    
    private func sendSpeedCommand(_ command: CarControlCommand) {
        // 使用连续速度控制协议 (功能码 0x10)
        let (speedX, speedY) = convertToSpeedComponents(command)
        
        // 将速度值转换为有符号8位整数的补码形式
        let speedXByte = convertToSignedByte(speedX)
        let speedYByte = convertToSignedByte(speedY)
        
        // 构建连续速度控制命令
        let speedData: [UInt8] = [speedXByte, speedYByte]
        let continuousSpeedCmd = CarProtocolBuilder.buildCommand(
            carType: 0x01,
            functionCode: 0x10,  // 连续速度控制功能码
            data: speedData
        )
        
        print("[AICommandExecutor] 发送连续速度命令: X=\(speedX), Y=\(speedY), 协议: \(String(data: continuousSpeedCmd, encoding: .utf8) ?? "编码失败")")
        
        tcpCarService?.sendData(continuousSpeedCmd) { success in
            print("[AICommandExecutor] 连续速度命令发送\(success ? "成功" : "失败")")
        }
    }
    
    private func getDirectionValue(_ direction: Direction) -> UInt8 {
        // 与人工控制页面保持一致的方向值
        switch direction {
        case .forward:
            return 1        // 前进
        case .backward:
            return 2        // 后退
        case .leftShift:
            return 3        // 左平移
        case .rightShift:
            return 4        // 右平移
        case .left:
            return 5        // 左旋
        case .right:
            return 6        // 右旋
        case .stop:
            return 0        // 停止
        case .horn:
            return 0        // 鸣笛不应该使用此方法，但为了完整性返回0
        }
    }
    
    private func convertToSpeedComponents(_ command: CarControlCommand) -> (Int, Int) {
        // 根据方向和速度级别计算X、Y速度分量
        let speedValue = Int(command.speed.value * 100) // 转换为0-100范围
        
        switch command.direction {
        case .forward:
            return (0, speedValue)   // 纯前进
        case .backward:
            return (0, -speedValue)  // 纯后退
        case .left:
            return (-speedValue, 0)  // 左旋转（原地）
        case .right:
            return (speedValue, 0)   // 右旋转（原地）
        case .leftShift:
            return (-speedValue, 0)  // 左平移（与左旋转速度分量相同，由硬件区分）
        case .rightShift:
            return (speedValue, 0)   // 右平移（与右旋转速度分量相同，由硬件区分）
        case .stop:
            return (0, 0)            // 停止
        case .horn:
            return (0, 0)            // 鸣笛不使用速度分量
        }
    }
    
    private func convertToSignedByte(_ value: Int) -> UInt8 {
        // 将-100到+100的值转换为有符号8位整数的补码形式
        let clampedValue = max(-100, min(100, value))
        
        if clampedValue >= 0 {
            // 正值: 0x00 到 0x64
            return UInt8(clampedValue)
        } else {
            // 负值: 0x9C 到 0xFF (补码)
            return UInt8(256 + clampedValue)
        }
    }
}

// MARK: - 扩展：命令序列管理
extension AICommandExecutor {
    // 获取当前执行状态描述
    var executionStatus: String {
        if !isExecuting {
            return "未执行"
        }
        
        if currentCommandIndex >= totalCommands {
            return "执行完成"
        }
        
        let currentCommand = commandQueue[safe: currentCommandIndex]
        if let command = currentCommand {
            return "执行中: \(command.direction.rawValue) \(command.speed.description)"
        }
        
        return "执行中..."
    }
    
    // 获取剩余命令数量
    var remainingCommands: Int {
        return max(0, totalCommands - currentCommandIndex)
    }
    
    // 检查是否可以暂停
    var canPause: Bool {
        return isExecuting && currentCommandIndex < totalCommands
    }
    
    // 检查是否可以恢复
    var canResume: Bool {
        return isExecuting && currentCommandIndex < totalCommands && currentTimer == nil
    }
}

// MARK: - 扩展：数组安全访问
extension Array {
    subscript(safe index: Index) -> Element? {
        return indices.contains(index) ? self[index] : nil
    }
} 